#include "Circle2.h"
#include "Disk2.h"
#include "Allocator.h"


////////////////////////////////////////////

const Disk2& Arc2::bounded() const
{
	assert(bounded_);
	return (*bounded_);
}

const Vector2 Arc2::startPoint() const
{
	return bounded_->supportPoint(startNormal_);
}

const Vector2 Arc2::endPoint() const
{
	return bounded_->supportPoint(endNormal_);
}

const Vector2 Arc2::startNormal() const
{
	return startNormal_;
}

const Vector2 Arc2::endNormal() const
{
	return endNormal_;
}

////////////////////////////////////////////

class ArcSegmentator2 : public IVertexIterator2
{
public:
	/** @pre angle is [0, pi] */
	ArcSegmentator2(const Arc2& arc, Scalar tolerance)
		: arc_(&arc)
		, current_(arc_->startNormal())
		, transform_(transformMatrix(arc_->bounded(), tolerance))
		, hasNext_(true)
	{
	}
	virtual const Vector2 point() const 
	{
		return arc_->bounded().supportPoint(current_);
	}

	virtual const Vector2 normal() const
	{
		return current_;
	}

	virtual bool next()
	{
		if (!hasNext_)
			return false;

		const Vector2 next(transform_ * current_); 

		if (det(next, arc_->endNormal()) > 0)
		{
			current_ = next;
			return true;
		}
		else
		{
			current_ = arc_->endNormal();
			hasNext_ = false;
			return true;
		}
	}

	/** @todo replace matrix by complex (provide rotation class) */
	static const Matrix2x2 transformMatrix(const Disk2& disk, Scalar tolerance)
	{
		const Scalar cosSemiAngle(1 - tolerance / disk.radius());
		const Scalar cosSemiAngle2(cosSemiAngle * cosSemiAngle);
		const Scalar sinSemiAngle2(1 - cosSemiAngle2);
		const Scalar sinSemiAngle(std::sqrt(sinSemiAngle2));
		const Scalar cosAngle = std::max(Scalar(0), 2 * cosSemiAngle2 - 1);//0-pi/2
		const Scalar sinAngle = 2 * cosSemiAngle * sinSemiAngle;

		return Matrix2x2(cosAngle, -sinAngle, sinAngle, cosAngle);
	}

private:
	const Arc2*	arc_;
	Vector2				current_;
	Matrix2x2			transform_;
	bool				hasNext_;
};

IVertexIterator2* Arc2::segmentate(Scalar tolerance, IStackAllocator& stack) const
{
	return stack.create<ArcSegmentator2>(*this, tolerance);
}

////////////////////////////////////////////
Circle2::Circle2(const Disk2& disk) : currentIndex_()
{
	static const Vector2 xPos(1, 0), yPos(0, 1), xNeg(-1, 0), yNeg(0, -1);
	edges_[0] = Arc2(disk, xPos, yPos);
	edges_[1] = Arc2(disk, yPos, xNeg);
	edges_[2] = Arc2(disk, xNeg, yNeg);
	edges_[3] = Arc2(disk, yNeg, xPos);
}

const IEdge2& Circle2::current() const
{
	return edges_[currentIndex_];
}

bool Circle2::next() 
{
	++currentIndex_;
	return (currentIndex_ < COUNT_EDGES);
}

////////////////////////////////////////////

